#include "stdafx.h"
#include "Solver.h"

using namespace std;

AssignmentProblem::AssignmentProblem(int cityCount, int** distances) {
    this->parent = NULL;
    this->cityCount = cityCount;
    this->distances = distances;
    this->constraintMap = new int[cityCount];
	for (int i = 0; i < cityCount; i++) {
		constraintMap[i] = 0;
	}
    this->exclusions = NULL;
	this->exclusionsCount = 0;
	this->lowerBound = infinity;
	this->patchedToCities = NULL;
	this->refCount = 0;
}

AssignmentProblem::AssignmentProblem(AssignmentProblem *parent, int index) {
    this->parent = parent;
    this->childIndex = index;
	this->constraintMap = NULL;
    this->exclusions = NULL;
	this->lowerBound = infinity;
	this->patchedToCities = NULL;
	this->refCount = 0;
	this->parent->IncrementRefCount();
}

void AssignmentProblem::IncrementRefCount() {
	this->refCount++;;
}

bool AssignmentProblem::DecrementRefCount() {
	this->refCount--;
	return (this->refCount == 0);
}

AssignmentProblem::~AssignmentProblem() {
	if (this->parent != NULL) {
		if (this->parent->DecrementRefCount())
			delete this->parent;
	}
	if (this->constraintMap != NULL) {
		delete[] this->constraintMap;
		if (this->exclusions != NULL)
			delete[] this->exclusions;
	    delete[] this->toCities;
	    delete[] this->fromCities;
	    delete[] this->u;
	    delete[] this->v;
	}

	if (this->patchedToCities != NULL) {
		delete[] this->patchedToCities;
		delete[] this->patchedFromCities;
	}

	// TODO: More deletions?
}

int* AssignmentProblem::GetCities() {
	return patchedToCities == NULL ? toCities : patchedToCities;
}

// 
// Solves the assignment problem and returns the total distance.
// 
long AssignmentProblem::Solve() {
    if (lowerBound < infinity)
        return lowerBound;
    bool success;
    if (parent == NULL)
        success = SolveFromScratch();
    else {
        AddConstraints();
        success = SolveIncremental();
    }

    if (!success)
        return infinity;

    // Compute total distance.
    this->lowerBound = 0;
    for (int i = 0; i < cityCount; i++)
        lowerBound += u[i] + v[i];

    // Extract cycles
    ComputeCycles();
    return lowerBound;
}

bool AssignmentProblem::IsAllowed(int row, int column) {
    int value = constraintMap[row];
    return (row != column) && ((value == 0) ? (row != column) : IsAllowedConstraint(value, column));
}
bool AssignmentProblem::IsAllowedConstraint(int value, int column) {
    if (value > 0)
        // Include
        return column == (value - 1);
    else {
        // Exclude
        value = -1 - value;
        while (exclusions[value] >= 0) {
            if (exclusions[value] == column)
                return false;
            value++;
        }
        return true;
    }
}

void AssignmentProblem::Include(int from, int to) {
    this->constraintMap[from] = 1 + to;
}

void AssignmentProblem::Exclude(int from, int to) {
    int index = this->constraintMap[from];
    if (index == 0) {
        // No constraints yet. Add this one at the end.
        this->constraintMap[from] = -1 - this->exclusionsCount;
		exclusions[exclusionsCount++] = to;
		exclusions[exclusionsCount++] = -1;
    } else if (index < 0) {
        // We have a constraint. Just insert this one at the front.
        for (int i = -1 - index; this->exclusions[i] != -1; i++) {
            if (this->exclusions[i] == to) {
                //Console.WriteLine("Oops! This shouldn't happen!");
                return;
            }
        }
		int first = -1 - index;
		memmove(this->exclusions + (first + 1), this->exclusions + first, 
			(this->exclusionsCount - first) * sizeof(int));
		this->exclusions[first] = to;
        for (int i = 0; i < this->cityCount; i++) {
            if (i == from)
                continue;
            if (this->constraintMap[i] < index)
                this->constraintMap[i]--;
        }
    }
}

bool AssignmentProblem::SolveFromScratch() {
    this->toCities = new int[cityCount];
    this->fromCities = new int[cityCount];
    this->u = new int[cityCount];
    this->v = new int[cityCount];

    // Label all cities as unassigned
    int assignedCities = 0;
    for (int i = 0; i < cityCount; i++) {
        toCities[i] = -1;
        fromCities[i] = -1;
		u[i] = 0;
		v[i] = 0;
    }

    // Find the smallest distance in each column.
    for (int column = 0; column < cityCount; column++) {
        // Find the i distance in the column.
        int minDistance = infinity;
        int minRow = -1;
        for (int row = 0; row < cityCount; row++) {
            if (!IsAllowed(row, column))
                continue;
            int distance = distances[row][column];
            if (distance < minDistance || (distance == minDistance && toCities[row] < 0)) {
                minRow = row;
                minDistance = distance;
            }
        }
        if (minRow < 0)
            return false;

        // This distance is the value of the dual variable of the column.
        v[column] = minDistance;

        // If the row is unassigned, we can assign it.
        if (toCities[minRow] < 0) {
            // Assign column to row minRow
            assignedCities++;
            fromCities[column] = minRow;
            toCities[minRow] = column;
            u[minRow] = 0;
        }
    }

    // Now look through unassigned rows, 
    // and see if they can be assigned.
    for (int row = 0; row < cityCount; row++) {
        if (toCities[row] >= 0)
            continue;

        int minReducedCost = infinity;
        int minColumn = -1;
        for (int column = 0; column < cityCount; column++) {
            if (!IsAllowed(row, column))
                continue;
            int reducedCost = distances[row][column] - v[column];
            if (reducedCost < minReducedCost
                || (reducedCost == minReducedCost && fromCities[column] < 0 && fromCities[minColumn] >= 0)) {
                minReducedCost = reducedCost;
                minColumn = column;
            }
        }

        if (minColumn < 0)
            return false;

        u[row] = minReducedCost;

        if (fromCities[minColumn] < 0) {
            // Assign minColumn to row i
            assignedCities++;
            toCities[row] = minColumn;
            fromCities[minColumn] = row;
        }
    }

    // For any remaining unassigned rows
    // we look for an augmenting path:
    if (assignedCities < cityCount) {
        AllocateAugmentingPathTemporaries();
        for (int row = 0; row < cityCount; row++) {
            if (toCities[row] >= 0)
                continue;
            FindAugmentingPath(row);
        }
        FreeAugmentingPathTemporaries();
    }
    return true;
}

void AssignmentProblem::AddConstraints() {
    this->cityCount = parent->cityCount;
    this->distances = parent->distances;

	// Copy parent constraints
	this->constraintMap = new int[this->cityCount];
	memcpy(this->constraintMap, parent->constraintMap, this->cityCount * sizeof(int));
	this->exclusions = new int[parent->exclusionsCount + 4];
	this->exclusionsLength = parent->exclusionsCount + 4;
	this->exclusionsCount = parent->exclusionsCount;
	if (parent->exclusionsCount > 0) {
		memcpy(this->exclusions, parent->exclusions, parent->exclusionsCount * sizeof(int));
	}

    // Add inclusions
    for (int i = 0; i < this->childIndex; i++)
        Include(parent->childFromCities[i], parent->childToCities[i]);
    // Add exclusions
    Exclude(parent->childFromCities[this->childIndex], parent->childToCities[this->childIndex]);
    if (this->childIndex > 0) {
        Exclude(parent->toCities[this->childIndex - 1], parent->fromCities[0]);
    }
}

bool AssignmentProblem::SolveIncremental() {
	this->toCities = new int[cityCount];
	memcpy(this->toCities, parent->toCities, cityCount * sizeof(int));
    this->fromCities = new int[cityCount];
	memcpy(this->fromCities, parent->fromCities, cityCount * sizeof(int));
    this->u = new int[cityCount];
	memcpy(this->u, parent->u, cityCount * sizeof(int));
    this->v = new int[cityCount];
	memcpy(this->v, parent->v, cityCount * sizeof(int));

    // Remove excluded edge.
    int row = parent->childFromCities[this->childIndex];
    int column = toCities[row];
    fromCities[column] = -1;
    toCities[row] = -1;

    AllocateAugmentingPathTemporaries();
    FindAugmentingPath(row);
    FreeAugmentingPathTemporaries();
    return true;
}

void AssignmentProblem::AllocateAugmentingPathTemporaries() {
    pathOrigins = new int[cityCount];
    reducedCosts = new int[cityCount];
    labeledRow = new int[cityCount];
    unlabeledColumn = new int[cityCount];
}
void AssignmentProblem::FreeAugmentingPathTemporaries() {
    delete[] pathOrigins;
    delete[] reducedCosts;
    delete[] labeledRow;
    delete[] unlabeledColumn;
}

void AssignmentProblem::FindAugmentingPath(int row) {
    labeledRow[0] = row;
    for (int column = 0; column < cityCount; column++) {
        if (!IsAllowed(row, column))
            reducedCosts[column] = infinity;
        else
            reducedCosts[column] = distances[row][column] - u[row] - v[column];
        pathOrigins[column] = row;
        unlabeledColumn[column] = column;
    }
    int unlabeledColumnCount = cityCount;
    int labeledRowCount = 1;

    for (; ; ) {
        // Find a zero in an unlabeled column
        bool foundZero = false;
        for (int l = 0; l < unlabeledColumnCount; l++) {
            int column = unlabeledColumn[l];
            if (reducedCosts[column] == 0) {
                foundZero = true;

                // If the column is unassigned, we're done.
                if (fromCities[column] < 0) {
                    do {
                        int origin = pathOrigins[column];
                        fromCities[column] = origin;
                        int hold = toCities[origin];
                        toCities[origin] = column;
                        column = hold;
                    } while (column >= 0);
                    return;
                }

                // label the row corresponding to the column.
                labeledRow[labeledRowCount++] = fromCities[column];
                // remove label from column column
                unlabeledColumn[l] = unlabeledColumn[--unlabeledColumnCount];
                break;
            }
        }
        if (foundZero) {
            // Scan labeled rows
            int r = labeledRow[labeledRowCount - 1];
            for (int l = 0; l < unlabeledColumnCount; l++) {
                int column = unlabeledColumn[l];
                if (!IsAllowed(r, column))
                    continue;
                int reducedCost = distances[r][column] - u[r] - v[column];
                if (reducedCost < reducedCosts[column]) {
                    reducedCosts[column] = reducedCost;
                    pathOrigins[column] = r;
                }
            }
        } else {
            // Update the dual variables
            // Find minimum value.
            int minReducedCost = infinity;
            for (int l = 0; l < unlabeledColumnCount; l++) {
                int column = unlabeledColumn[l];
                if (reducedCosts[column] < minReducedCost)
                    minReducedCost = reducedCosts[column];
            }
            // Add minimum to dual variables of labeled rows
            for (int l = 0; l < labeledRowCount; l++) {
                int r = labeledRow[l];
                u[r] += minReducedCost;
            }
            // Subtract minimum from dual variables of labeled columns
            for (int column = 0; column < cityCount; column++) {
                if (reducedCosts[column] == 0)
                    v[column] -= minReducedCost;
                else
                    reducedCosts[column] -= minReducedCost;
            }
        }

    }
}

// Gets the starting point of the cycle of i length.
void AssignmentProblem::ComputeCycles() {
    bool* visited = new bool[cityCount];
    for (int i = 0; i < cityCount; i++) 
		visited[i] = false;
	this->cycles = std::vector<Cycle>();
    int cycleCount = 0;
    for (int i = 0; i < cityCount; i++) {
        if (visited[i])
            continue;
        cycleCount++;
        visited[i] = true;
        int next = toCities[i];
        int length = 1;
        int freeLength = constraintMap[i] > 0 ? 0 : 1;
        while (next != i) {
            visited[next] = true;
            length++;
            if (constraintMap[next] <= 0)
                freeLength++;
            next = toCities[next];
        }
        Cycle cycle;
        cycle.Length = length;
        cycle.FreeLength = freeLength;
        cycle.StartNode = i;
        this->cycles.push_back(cycle);
    }

	delete [] visited;

	sort(cycles.begin(), cycles.end());

    int minFreeLength = MAXINT;
    for (unsigned int i = 0; i < cycles.size(); i++)
        if (cycles[i].FreeLength < minFreeLength) {
            minCycle = i;
            minFreeLength = cycles[i].FreeLength;
        }
}

void AssignmentProblem::Patch(int from, int to, int from2, int to2) {
    patchedToCities[from] = to2;
    patchedFromCities[to2] = from;
    patchedToCities[from2] = to;
    patchedFromCities[to] = from2;
}

// Patches the cycles with the specified start cities using the
// Karp-Steele patch algorithm.
// Returns the cost of the patch.
long AssignmentProblem::Patch(int startCity, int startCity2) {
    int bestLast = -1;
    int bestLast2 = -1;
    int last = startCity;
    int next = toCities[last];
    long minPenalty = MAXLONG;
    while (next != startCity) {

        long basePenalty = -distances[last][next];
        int last2 = startCity2;
        int next2 = toCities[last2];
        while (next2 != startCity2) {
            if (IsAllowed(last, next2) && IsAllowed(last2, next)) {
                long penalty = basePenalty - distances[last2][next2]
                    + distances[last][next2] + distances[last2][next];
                if (penalty < minPenalty) {
					if (penalty == 0) {
                        Patch(last, next, last2, next2);
                        return 0;
					}
                    minPenalty = penalty;
                    bestLast = last;
                    bestLast2 = last2;
                }
            }
            last2 = next2;
            next2 = toCities[last2];
        }
        last = next;
        next = toCities[last];
    }

    if (bestLast < 0 || bestLast2 < 0)
        return infinity;
    Patch(bestLast, patchedToCities[bestLast], bestLast2, patchedToCities[bestLast2]);
    return minPenalty;
}

long AssignmentProblem::Patch(long upperBound) {
    patchedToCities = new int[cityCount];
	memcpy(patchedToCities, toCities, cityCount * sizeof(int));
    patchedFromCities = new int[cityCount];
	memcpy(patchedFromCities, fromCities, cityCount * sizeof(int));

    long penalty = 0;
    long maxPenalty = upperBound - this->lowerBound;
    for (size_t cycle = cycles.size() - 1; cycle > 0; --cycle) {
        int startCity = cycles[cycle].StartNode;
        int startCity2 = cycles[cycle - 1].StartNode;
        penalty += Patch(startCity, startCity2);
        if (penalty >= maxPenalty)
            break;
    }

    return penalty;
}

bool AssignmentProblem::operator<(AssignmentProblem const &y) {
    if (getLowerBound() < y.getLowerBound())
        return true;
    if (getLowerBound() > y.getLowerBound())
        return false;
    if (getChildCount() < y.getChildCount())
        return true;
    if (getChildCount() > y.getChildCount())
        return false;
    return false;
}

void AssignmentProblem::GenerateSubproblems(list<AssignmentProblem*> &problemList) {
    if (this->cycles.size() == 0)
        return;

    int minLength = cycles[minCycle].FreeLength;
    childFromCities = new int[minLength];
    childToCities = new int[minLength];

    int last = cycles[minCycle].StartNode;
    int next;
    int index = 0;
    do {
        next = toCities[last];
        if (constraintMap[last] <= 0) {
            childFromCities[index] = last;
            childToCities[index++] = next;
        }
        last = next;
    } while (last != cycles[minCycle].StartNode);

    for (int i = 0; i < minLength; i++) {
		AssignmentProblem *newProblem = new AssignmentProblem(this, i);
        problemList.push_back(newProblem);
    }
}


Solver :: Solver(Configurator *configurator)
{
	dwarfConfigurator = configurator;					// process settings from the command line 
	dwarfConfigurator -> WriteSettings();
	distances = dwarfConfigurator -> GetData(cityCount);// getting the array of edge values
    best = new AssignmentProblem(cityCount, distances);
}

void Solver :: ArrayDestroyer(int **arrayForDel, int dimension)
{
	for (int i = 0; i < dimension; i++)
	{
		free(arrayForDel[i]);
	}
	free(arrayForDel);
}

// Adds a solved problem to the solved list, keeping the solved list sorted.
void Solver::AddSolvedProblem(AssignmentProblem *problem) {
	problem->IncrementRefCount();
	list<AssignmentProblem*>::iterator index;
	for (index = solvedList.begin(); index != solvedList.end() && *problem < **index; index++)
        ;
    solvedList.insert(index, problem);
}

AssignmentProblem *Solver::NextSolvedProblem() {
    // Prune solved problems
    while (solvedList.size() > 0 && solvedList.front()->getLowerBound() >= upperBound) {
		AssignmentProblem *front = solvedList.front();
        solvedList.pop_front();
		if (front->DecrementRefCount())
			delete front;
    }
    if (solvedList.size() > 0) {
        // solvedList.sort();
    	AssignmentProblem *problem = solvedList.back();
		solvedList.pop_back();
    	return problem;
	} else {
		return NULL;
	}
}

//Non-parallelized method of solving
int Solver::Solve() {
    best->Solve();
    if (best->getCycleCount() == 1)
        upperBound = best->getLowerBound();
    else {
        upperBound = best->getLowerBound() + best->Patch(infinity);
        best->GenerateSubproblems(problemList);
    }
	best->IncrementRefCount();
	totalNodes = 1;
    for (; ; ) {
		while (problemList.size() > 0) {
			AssignmentProblem* problem = problemList.front();
			problemList.pop_front();
            if (problem->getParentLowerBound() >= upperBound)
                continue;
            long lowerBound = problem->Solve(); totalNodes++;
            if (problem->getCycleCount() == 1) {
                if (lowerBound < upperBound) {
					if (best->DecrementRefCount())
						delete best;
                    best = problem;
					best->IncrementRefCount();
                    upperBound = lowerBound;
					printf("\n\r%ld", upperBound);
                }
            } else if (lowerBound >= upperBound) {
                continue;
            } else {
                long penalty = problem->Patch(upperBound);
                long newUpperBound = penalty == infinity ? infinity : lowerBound + penalty;
                if (newUpperBound < upperBound) {
					if (best->DecrementRefCount())
						delete best;
                    best = problem;
					best->IncrementRefCount();
                    upperBound = newUpperBound;
					printf("\n\r%ld", upperBound);
                }
                AddSolvedProblem(problem);
            }
        }
        problemList.erase(problemList.begin(), problemList.end());
        if (solvedList.size() > 0) {
            AssignmentProblem *problem = NextSolvedProblem();
            if (problem == NULL || problem->getLowerBound() >= upperBound) {
                continue;
            }
            problem->GenerateSubproblems(problemList);
			if (problem->DecrementRefCount())
				delete problem;
        } else
            break;
    }
	return 0;
}

// Problem results output
void Solver::Finish(double time) {
    dwarfConfigurator->Close(time, this->cityCount, best->GetCities());
    printf("Total Distance    : %ld\r\n", this->upperBound);
	printf("# nodes           : %ld\r\n", this->totalNodes);
	ArrayDestroyer(distances, cityCount);
	delete best;
}
